#include "GenerateSurface.h"
#include "HeightMap.h"
#include "SharedConstants.h"
#include "HeightMapCoordinates.h"
#include <limits>
#include "TransformCoords.h"

namespace
  {
  void _GetNormalizedNormalVector(double& o_n, double& o_m, double& o_l, const Point3DID& i_point, const Model3D& i_model)
    {
    TVector3 normals_sum = TVector3(0, 0, 0);
    size_t normal_counter = 0;

    auto triangles = i_model.GetNeighborTriangles(i_point);
    for(auto it = triangles.begin(); it != triangles.end(); ++it)
      {
      // Finding normal vector
      // names for points
      const TVector3 A = Coords::WorldToUI((*it)->GetPoint(0));
      const TVector3 B = Coords::WorldToUI((*it)->GetPoint(1));
      const TVector3 C = Coords::WorldToUI((*it)->GetPoint(2));

      // vector product [AB;AC]
      const TVector3 AB = TVector3(B.X() - A.X(), B.Y() - A.Y(), B.Z() - A.Z());
      const TVector3 AC = TVector3(C.X() - A.X(), C.Y() - A.Y(), C.Z() - A.Z());

      // normal vector is a koefs in vector product
      double n = 0, m = 0, l = 0;
      n = AB.Y() * AC.Z() - AB.Z() * AC.Y(); 
      m = AB.Z() * AC.X() - AB.X() * AC.Z(); 
      l = AB.X() * AC.Y() - AB.Y() * AC.X(); 

      // normal vector -> N = {n, m, l}
      // normalizing vector
      double length = sqrt(n*n + m*m + l*l);
      double inv_length = 1.0 / length;
      n *= inv_length;
      m *= inv_length;
      l *= inv_length;

      // adding normal to vector of normals
      normals_sum += TVector3(n, m, l);

      // incrementing normal_counter
      ++normal_counter;
      }
    // getting average and setting output value
    o_n = normals_sum.X() / static_cast<double>(normal_counter);
    o_m = normals_sum.Y() / static_cast<double>(normal_counter);
    o_l = normals_sum.Z() / static_cast<double>(normal_counter);
    }
  }

/*
*
* @params:
* i_height_map - HeightMap to generate surface from
* i_origin_x - x coordinate of Base-point(origin)
* i_origin_y - y coordinate of Base-point(origin)
* i_origin_z - z coordinate of Base-point(origin)
* i_length - Length of HeightMap's cell (cell = square) 
* 
**/
void GenerateSurface(Model3D &o_model, const HeightMap& i_height_map, double i_origin_x, double i_origin_y,
                     double i_origin_z, double i_length)
  {
  o_model.Clear();

  //Generating points from HeightMap
  std::vector< std::vector<Point3DID> > points;
  points.reserve(i_height_map.GetSizeX());

  std::vector<Point3DID> temp_vector_of_points;
  temp_vector_of_points.reserve(i_height_map.GetSizeY());

  for(size_t x = 0; x < i_height_map.GetSizeX(); ++x)
    {
    for(size_t y = 0; y < i_height_map.GetSizeY(); ++y)
      {  
      double _x = x * i_length + i_origin_x;
      double _y = y * i_length + i_origin_y;
      double _z = i_height_map.Get(x, y) * i_length + i_origin_z;
      if (_z > GroundConstants::MAX_HILL_HEIGHT)
        _z = GroundConstants::MAX_HILL_HEIGHT;
      if (_z < GroundConstants::MAX_HOLE_DEPTH)
        _z = GroundConstants::MAX_HOLE_DEPTH;

      Point3DID id = o_model.AddPoint(_x, _y, _z);
      temp_vector_of_points.push_back(id);
      }

    points.push_back(temp_vector_of_points);
    temp_vector_of_points.clear();
    }

  /*
  * Generating triangles
  */
  for(size_t x = 0; x < i_height_map.GetSizeX() - 1; ++x)
    {
    for(size_t y = 0; y < i_height_map.GetSizeY() - 1; ++y)
      {
      //we are going counterclockwise
      /*
      * 1 2
      * 3 4
      *
      * 1st triangle - 3-1-2
      * 2nd triangle - 4-3-2
      **/
      Point3DID m_a = points[x+1][y];
      Point3DID m_b = points[x][y];
      Point3DID m_c = points[x][y+1];
   
      Triangle3DID tr1_id = o_model.AddTriangle(m_a, m_b, m_c);
      
      m_a = points[x+1][y+1];
      m_b = points[x+1][y];
      m_c = points[x][y+1];

      Triangle3DID tr2_id = o_model.AddTriangle(m_a, m_b, m_c);
      }
    }

  // Calculating normals

  for(auto it = o_model.TrianglesMutable().begin(); it != o_model.TrianglesMutable().end(); it++)
    {
    TVector3 normals[3];
    for (size_t i = 0; i < 3; ++i)
      {
      double n = 0.0, m = 0.0, l = 0.0;
      _GetNormalizedNormalVector(n, m, l, it->GetPointId(i), o_model);
      normals[i] = TVector3(n, m, l);
      }
    it->SetPointNormals(normals[0], normals[1], normals[2]);
    }
  }


void ApplyGroundTypes(Model3D& io_model, const HeightMap& i_height_map, double i_origin_x, double i_origin_y,
                      double i_origin_z, double i_length, const TGroundTypeMap& i_ground_types)
  {
  if( i_ground_types.size()+1 != i_height_map.GetSizeX() &&
    i_ground_types[0].size()+1 != i_height_map.GetSizeY() )
    throw std::invalid_argument("Ground Types Map is empty or corrupted!");

  io_model.SetMaterialName("Test/ColourTest");

  
  for(auto iterator = io_model.Triangles().begin(); iterator != io_model.Triangles().end(); iterator++)
    {
    TVector3 p0 = iterator->GetPoint(0);
    TVector3 p1 = iterator->GetPoint(1);
    TVector3 p2 = iterator->GetPoint(2);
    size_t x = static_cast<size_t>(p0.X() + p1.X() + p2.X()) / 3;
    size_t y = static_cast<size_t>(p0.Y() + p1.Y() + p2.Y()) / 3;

    x = static_cast<size_t>((x - i_origin_x) / i_length);
    y = static_cast<size_t>((y - i_origin_y) / i_length);

    if(i_ground_types[x][y] == GT_GRASS)
      {
      if(p0.Y() == p1.Y() && p1.X() == p2.X())
        const_cast<Triangle3D&>(*iterator).SetPointsUV(TVector2(0, 1), TVector2(0, 0), TVector2(0.5, 0));
      else
        const_cast<Triangle3D&>(*iterator).SetPointsUV(TVector2(0.5, 1), TVector2(0, 1), TVector2(0.5, 0));
      }
    else
      {
      if(i_ground_types[x][y] == GT_WALKWAY) 
        {
        if(p0.Y() == p1.Y() && p1.X() == p2.X())
          const_cast<Triangle3D&>(*iterator).SetPointsUV(TVector2(0.5, 1), TVector2(0.5, 0), TVector2(1, 0));
        else
          const_cast<Triangle3D&>(*iterator).SetPointsUV(TVector2(1, 1), TVector2(0.5, 1), TVector2(1, 0));
        }
      }    
    }
  }


void GeneratePartOfSurface(Model3D &o_model, std::vector<TVector3>& i_changed_points, double i_origin_x, double i_origin_y,
                     double i_origin_z, double i_length)
  {
  o_model.Clear();

  //we find the points with min and max x-coords
  auto min_max_x = std::minmax_element(i_changed_points.begin(), i_changed_points.end(), [](const TVector3& i_point1, const TVector3& i_point2) -> int
    {
     if(i_point1.X() < i_point2.X())
       return true;
     else 
       return false;
    });
  //we find the points with min and max y-coords
  auto min_max_y = std::minmax_element(i_changed_points.begin(), i_changed_points.end(), [](const TVector3& i_point1, const TVector3& i_point2) -> int
    {
     if(i_point1.Y() < i_point2.Y())
       return true;
     else 
       return false;
    });
  
  // now we find indexes of the border points in the height map 
  auto min_x_indexes = HeightMapUtils::FindHeightMapIndexes(TVector2(min_max_x.first->X(), min_max_x.first->Y()),
    TVector2(GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Y), GroundConstants::CELL_SIZE);
  
  auto max_x_indexes = HeightMapUtils::FindHeightMapIndexes(TVector2(min_max_x.second->X(), min_max_x.second->Y()),
    TVector2(GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Y), GroundConstants::CELL_SIZE);
  
  auto min_y_indexes = HeightMapUtils::FindHeightMapIndexes(TVector2(min_max_y.first->X(), min_max_y.first->Y()),
    TVector2(GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Y), GroundConstants::CELL_SIZE);
  
  auto max_y_indexes = HeightMapUtils::FindHeightMapIndexes(TVector2(min_max_y.second->X(), min_max_y.second->Y()),
    TVector2(GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Y), GroundConstants::CELL_SIZE);
  
  
  //So when we construct points and triangles of the model the x-coord of the point contains its values in 
  //[min_x_indexes.first, max_x_indexes.first] segment and the y-coord in 
  //[min_y_indexes.second, max_y_indexes.second] segment relatievly.
  //It helps us to decrease the amount of the loop iterations and to avoid 
  //unnecessary points getting in o_model.
  HeightMap heightmap(GroundConstants::HEIGHTMAP_SIZE, GroundConstants::HEIGHTMAP_SIZE);
  for(size_t i = min_x_indexes.first; i <= max_x_indexes.first; ++i)
    for(size_t j = min_y_indexes.second; j <= max_y_indexes.second; ++j)
      heightmap.Set(i, j, GroundConstants::MAX_HOLE_DEPTH - 1);

  
  for(auto it = i_changed_points.begin(); it != i_changed_points.end(); ++it)
    {
    std::pair<size_t, size_t> coords = HeightMapUtils::FindHeightMapIndexes(TVector2(it->X(), it->Y()), TVector2(GroundConstants::ORIGIN_X,
                                                       GroundConstants::ORIGIN_Y), GroundConstants::CELL_SIZE);
  	heightmap.Set(coords.first, coords.second, it->Z());
    }

  //Generating points from HeightMap
  std::vector< std::vector<Point3DID> > points;
  std::vector< std::vector<TVector3> > changeable_points;

  for(size_t x = min_x_indexes.first; x <= max_x_indexes.first; ++x)
    {
    std::vector<Point3DID> temp_vector_of_points;
    std::vector<TVector3> temp_changeable_points;
    for(size_t y = min_y_indexes.second; y <= max_y_indexes.second; ++y)
      {  
      double _x = x * i_length + i_origin_x;
      double _y = y * i_length + i_origin_y;
      double _z = heightmap.Get(x, y) * i_length + i_origin_z + 8;
      Point3DID id = o_model.AddPoint(_x, _y, _z);
      temp_vector_of_points.push_back(id);
      temp_changeable_points.push_back(TVector3(_x,_y,_z));
      }

    points.push_back(temp_vector_of_points);
    changeable_points.push_back(temp_changeable_points);
    }

  /*
  * Generating triangles
  */
  for(size_t x = min_x_indexes.first; x < max_x_indexes.first; ++x)
    {
    for(size_t y = min_y_indexes.second; y < max_y_indexes.second; ++y)
      {
      //we are going clockwise
      /*
      * 1 2
      * 3 4
      *
      * 1st triangle - 3-1-2
      * 2nd triangle - 4-3-2
      **/
      
      if(changeable_points[x + 1 - min_x_indexes.first][y - min_y_indexes.second].Z() < GroundConstants::MAX_HOLE_DEPTH ||
        changeable_points[x - min_x_indexes.first][y - min_y_indexes.second].Z() < GroundConstants::MAX_HOLE_DEPTH ||
        changeable_points[x - min_x_indexes.first][y+1 - min_y_indexes.second].Z() < GroundConstants::MAX_HOLE_DEPTH )
        {}
      else
        Triangle3DID tr1_id = o_model.AddTriangle(points[x+1 - min_x_indexes.first][y - min_y_indexes.second],
                                                  points[x - min_x_indexes.first][y - min_y_indexes.second],
                                                  points[x - min_x_indexes.first][y+1 - min_y_indexes.second]);

      if(changeable_points[x+1 - min_x_indexes.first][y+1 - min_y_indexes.second].Z() < GroundConstants::MAX_HOLE_DEPTH ||
        changeable_points[x+1 - min_x_indexes.first][y - min_y_indexes.second].Z() < GroundConstants::MAX_HOLE_DEPTH ||
        changeable_points[x - min_x_indexes.first][y+1 - min_y_indexes.second].Z() < GroundConstants::MAX_HOLE_DEPTH )
        {}
      else
        Triangle3DID tr2_id = o_model.AddTriangle(points[x+1 - min_x_indexes.first][y+1 - min_y_indexes.second], 
                                                  points[x+1 - min_x_indexes.first][y - min_y_indexes.second], 
                                                  points[x - min_x_indexes.first][y+1 - min_y_indexes.second]);
      }
    }
  }

